/****************************** (C) COPYRIGHT 2016 Rsestudio ****************************
* File Name          : 
* Author             : RSES_Liyang
* Version            : V1.0.0
* Date               : 00-May-20
* Description        : 
*****************************************************************************************/
/* Includes ----------------------------------------------------------------------------*/
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "Bsp.h"
#include "main.h"
#include "device_pins.h"
#include "vibration.h"
/* Extern variables --------------------------------------------------------------------*/
/* Private define ----------------------------------------------------------------------*/
/* Private macro -----------------------------------------------------------------------*/
/* Private typedef ---------------------------------------------------------------------*/
/* Private variables -------------------------------------------------------------------*/
/* Private functions -------------------------------------------------------------------*/


void vib_nop(void)
{
	uint32_t i;
	
//	for(i=0;i<2;i++)
//	{
    __asm("nop");
    __asm("nop");
//	}
}



#if (USE_VIBRATE == 1)
	
    
void vib_writeByte(uint8_t addr,uint8_t Data)
{
    uint8_t i;
    uint8_t Temp;
	
	ADS_SCLK_H();
	vib_nop();
	
	ACC_CS_L();
	vib_nop();
	
	Temp = (addr&0x7F)|(0<<7);
    for(i=0;i<8;i++)
    {   
		ADS_SCLK_L();
		vib_nop();		
        if(Temp&0x80)
        {
            ADS_MOSI_H();
        }
        else
        {
            ADS_MOSI_L();
        }
		
        Temp<<=1;
        ADS_SCLK_H();
		vib_nop();		
    }
	
	Temp = Data;
	for(i=0;i<8;i++)
    {   
		ADS_SCLK_L();
		vib_nop();
		
        if(Temp&0x80)
        {
            ADS_MOSI_H();
        }
        else
        {
            ADS_MOSI_L();
        }
		
        Temp<<=1;
        ADS_SCLK_H();
		vib_nop();		
    }
	ACC_CS_H();
	vib_nop();
	ADS_SCLK_L();
}

uint8_t vib_readByte(uint8_t addr)
{
    uint8_t i;
    uint8_t Temp;
	uint8_t recv_data;
	recv_data = 0;
	
	ADS_SCLK_H();
	vib_nop();
	
	ACC_CS_L();
	vib_nop();
	
	Temp = (addr&0x7F)|(1<<7);
    for(i=0;i<8;i++)
    {   
		ADS_SCLK_L();
		vib_nop();		
        if(Temp&0x80)
        {
            ADS_MOSI_H();
        }
        else
        {
            ADS_MOSI_L();
        }
		
        Temp<<=1;
        ADS_SCLK_H();
		vib_nop();		
    }
	
	for(i=0;i<8;i++)
    {   
		ADS_SCLK_L();
		vib_nop();
			
        ADS_SCLK_H();
		vib_nop();	
		
		recv_data<<=1;
		if(ADS_MISO() != 0)
        {
            recv_data |= 0x01;
        }	
    }
	
	ACC_CS_H();
	vib_nop();
	ADS_SCLK_L();
	return recv_data;
}

uint16_t vib_read_short(uint8_t addr)
{
    uint8_t i;
    uint8_t Temp;
	uint8_t recv_data_L,recv_data_H;
	recv_data_L = 0;
	recv_data_H = 0;
    
	ADS_SCLK_H();
	vib_nop();
	
	ACC_CS_L();
	vib_nop();
	
	Temp = (addr&0x7F)|(1<<7);
    for(i=0;i<8;i++)
    {   
		ADS_SCLK_L();
		vib_nop();		
        if(Temp&0x80)
        {
            ADS_MOSI_H();
        }
        else
        {
            ADS_MOSI_L();
        }
		
        Temp<<=1;
        ADS_SCLK_H();
		vib_nop();		
    }
	
	for(i=0;i<8;i++)
    {   
		ADS_SCLK_L();
		vib_nop();
			
        ADS_SCLK_H();
		vib_nop();	
		
		recv_data_L<<=1;
		if(ADS_MISO() != 0)
        {
            recv_data_L |= 0x01;
        }	
    }
    
    for(i=0;i<8;i++)
    {   
		ADS_SCLK_L();
		vib_nop();
			
        ADS_SCLK_H();
		vib_nop();	
		
		recv_data_H<<=1;
		if(ADS_MISO() != 0)
        {
            recv_data_H |= 0x01;
        }	
    }
	
	ACC_CS_H();
	vib_nop();
	ADS_SCLK_L();
	return (recv_data_H<<8) | recv_data_L;
}

int angle_low_filter_alpha;
uint8_t vib_raw_data_buf[24];
kalman_filter_t vib_kalman[2];

#define VIB_DISP_AVG_BUF_LENGTH         60

#define VIB_PAKE_LENGTH                 60

uint16_t fifo_status;

int vib_high_filter_alpha;

int vib_avg_buf_avg_x[VIB_DISP_AVG_BUF_LENGTH];
int vib_avg_buf_clc_x[VIB_DISP_AVG_BUF_LENGTH];

int vib_avg_buf_avg_y[VIB_DISP_AVG_BUF_LENGTH];
int vib_avg_buf_clc_y[VIB_DISP_AVG_BUF_LENGTH];

int vib_avg_buf_avg_z[VIB_DISP_AVG_BUF_LENGTH];
int vib_avg_buf_clc_z[VIB_DISP_AVG_BUF_LENGTH];


#define VIB_AVG_RESULT_BUF_LENGTH         40

int vib_avg_result_buf_avg_x[VIB_AVG_RESULT_BUF_LENGTH];
int vib_avg_result_buf_clc_x[VIB_AVG_RESULT_BUF_LENGTH];

int vib_avg_result_buf_avg_y[VIB_AVG_RESULT_BUF_LENGTH];
int vib_avg_result_buf_clc_y[VIB_AVG_RESULT_BUF_LENGTH];

int vib_avg_result_buf_avg_z[VIB_AVG_RESULT_BUF_LENGTH];
int vib_avg_result_buf_clc_z[VIB_AVG_RESULT_BUF_LENGTH];

short vib_max_list[VIB_PAKE_LENGTH];
short vib_min_list[VIB_PAKE_LENGTH];

short vib_sign[VIB_PAKE_LENGTH];
char  vib_flag_max[VIB_PAKE_LENGTH];
char  vib_idelete[VIB_PAKE_LENGTH];
char  vib_temp_max[VIB_PAKE_LENGTH];

short vib_max_list_cnt;
short vib_min_list_cnt;
/*****************************************************************************************
* Function Name  : IIC_PortInit
* Description    : 
* Input          : None
* Output         : None
* Return         : None
*****************************************************************************************/
uint8_t vibration_init(void)
{
    
    average_filter_init(&iProfile.vibrate.avg_vib_x,vib_avg_buf_avg_x,vib_avg_buf_clc_x,VIB_DISP_AVG_BUF_LENGTH,0,0);
    average_filter_init(&iProfile.vibrate.avg_vib_y,vib_avg_buf_avg_y,vib_avg_buf_clc_y,VIB_DISP_AVG_BUF_LENGTH,0,0);
    average_filter_init(&iProfile.vibrate.avg_vib_z,vib_avg_buf_avg_z,vib_avg_buf_clc_z,VIB_DISP_AVG_BUF_LENGTH,0,0);
    
    average_filter_init(&iProfile.vibrate.avg_vib_result_x,vib_avg_result_buf_avg_x,vib_avg_result_buf_clc_x,VIB_AVG_RESULT_BUF_LENGTH,5,0);
    average_filter_init(&iProfile.vibrate.avg_vib_result_y,vib_avg_result_buf_avg_y,vib_avg_result_buf_clc_y,VIB_AVG_RESULT_BUF_LENGTH,5,0);
    average_filter_init(&iProfile.vibrate.avg_vib_result_z,vib_avg_result_buf_avg_z,vib_avg_result_buf_clc_z,VIB_AVG_RESULT_BUF_LENGTH,5,0);   

    
    vib_writeByte(0x12,0xB5);//CTRL3_C  : BOOT=0;DataUpdate=continuous;INT_Poly=Low;INT_Out=OD;SPI_Line=4line;ADDR_INC=Enable;SW_RESET=0;
    
    vTaskDelay(pdMS_TO_TICKS(300));
    
    vib_writeByte(0x18,0xE1);//CTRL9_XL ://------------------
    
    vib_raw_data_buf[0] = vib_readByte(0x0F);
	vib_raw_data_buf[0] = vib_readByte(0x15);
    
    vib_writeByte(0x02,0x7F);//PIN_CTRL : SDO_PU=1;
    vib_writeByte(0x07,0xFF);//FIFO_CTRL1 : FIFO=40;
    vib_writeByte(0x08,0x00);//FIFO_CTRL2 : 
    vib_writeByte(0x09,0x07);//FIFO_CTRL3 : FIFO Freq=833Hz
    vib_writeByte(0x0A,0x06);//FIFO_CTRL4 : 
    vib_writeByte(0x0B,0x40);//FIFO_CTRL4 : 
    vib_writeByte(0x0C,0x00);//FIFO_CTRL4 : //---
    vib_writeByte(0x0D,0x00);//INT1_CTRL : 
    vib_writeByte(0x0E,0x00);//INT1_CTRL :
    
    vib_writeByte(0x10,0x70);//CTRL1_XL : ODR=208Hz;FS=2G;LPF2=0 //0x70
    vib_writeByte(0x11,0x00);//CTRL2_G  : ODR=208Hz;FS=250dps
    vib_writeByte(0x12,0xB4);//CTRL3_C  : BOOT=0;DataUpdate=continuous;INT_Poly=Low;INT_Out=OD;SPI_Line=4line;ADDR_INC=Enable;SW_RESET=0;
    vib_writeByte(0x13,0x04);//CTRL4_C  : SLEEP=0;INT_PINS=0;DRDY=0;I2C_EN=DISABLE;LPF1_SEL_G=0;
    vib_writeByte(0x14,0x00);//CTRL5_C  : //self test
    vib_writeByte(0x15,0x00);//CTRL6_G  :
    vib_writeByte(0x16,0x00);//CTRL7_G  :
    vib_writeByte(0x17,0x00);//CTRL8_XL :
    //vib_writeByte(0x18,0xE0);//CTRL9_XL ://------------------
    vib_writeByte(0x19,0x00);//CTRL10_XL:
    
    vib_writeByte(0x56,0x00);//INT_CFG0 :
    vib_writeByte(0x57,0x00);//INT_CFG1 :
    
    vib_writeByte(0x73,0x00);//X_OFS_USR :
    vib_writeByte(0x74,0x00);//Y_OFS_USR :
    vib_writeByte(0x75,0x00);//Z_OFS_USR :
    
    
    vib_raw_data_buf[1] = vib_readByte(0x1E);   //STATUS_REG
    vib_raw_data_buf[2] = vib_readByte(0x20);   //TEMP_L
    vib_raw_data_buf[3] = vib_readByte(0x21);   //TEMP_H
    
    vib_writeByte(0x18,0xE0);//CTRL9_XL ://------------------
    
    kalmanFilter_init(&vib_kalman[0],0,0.05,0.9);
    kalmanFilter_init(&vib_kalman[1],0,0.05,0.9);
    
    angle_low_filter_alpha = 18;
    vib_high_filter_alpha = 80;
    
//	vib_test_buf[1] = vib_readByte(0x2C);
//	
//	
//	vib_writeByte(0x2E,0x00);
//	vib_writeByte(0x31,0x0B);
//	vib_writeByte(0x2D,0x08);
//	vib_writeByte(0x2C,0x0B);
//	vib_writeByte(0x38,0x8A);
//	
//	vib_writeByte(0x1E,0x00);
//	vib_writeByte(0x1F,0x00);
//	vib_writeByte(0x20,0x00);
//	
//	
//	vib_readxyz(vib_test_buf);


	return 0;
}
#define VIB_PP_BUF_CNT          5

int32_t vib_pp_point;
int32_t vib_pp_buf[3][VIB_PP_BUF_CNT];

int angle_pitch_low_pass_prev;
int angle_roll_low_pass_prev;

int vib_x_high_pass_prev;
int vib_y_high_pass_prev;
int vib_z_high_pass_prev;

uint16_t polling_cnt;

int vibration_polling(void)
{
    uint8_t i,j,k;
    uint8_t Temp;
	uint8_t recv_data;
    
    int16_t temp_x,temp_y,temp_z;
    
    int32_t ax,ay,az;
    int32_t ax2,ay2,az2;
    int32_t vib_max[3];
    int32_t vib_min[3];
    
    int16_t fifo_level;
    int16_t fifo_status;
    
    int32_t temp_32;
    
    uint16_t peak_cnt;
    //vib_writeByte(0x0A,0x00);//FIFO_CTRL4 :
    
//    fifo_status = vib_readByte(0x3A);
//    fifo_status |= vib_readByte(0x3B)<<8;

    ax = 0;
    ay = 0;
    az = 0;  
    
    ACC_CS_H();  
    ADS_CS_1_H();
	ADS_CS_2_H();
	ADS_CS_3_H();
	ADS_CS_4_H();
    ADS_CS_5_H();
	ADS_CS_6_H();
    ADS_CS_7_H();
	ADS_CS_8_H(); 
    vib_nop();	
    
    fifo_status = vib_read_short(0x3A);
    fifo_level = fifo_status&0x3FF;
    
    if((fifo_status & (0x60<<8))!=0)
    {
        vib_writeByte(0x0A,0x00);//FIFO_CTRL4 :
        vib_nop();
        vib_nop();
        vib_writeByte(0x0A,0x06);//FIFO_CTRL4 :
        vib_nop();
        vib_nop();
        
        return -1;
    }

    if(fifo_level > 60)
        fifo_level = 60;
    
    for(k=0;k<fifo_level;k++)
    {
        ADS_SCLK_H();
        vib_nop();
        
        ACC_CS_L();
        vib_nop();
        
        Temp = 0X78|(1<<7);
        for(i=0;i<8;i++)
        {   
            ADS_SCLK_L();
            vib_nop();		
            if(Temp&0x80)
            {
                ADS_MOSI_H();
            }
            else
            {
                ADS_MOSI_L();
            }
            
            Temp<<=1;
            ADS_SCLK_H();
            vib_nop();		
        }
        
        ADS_MOSI_L();
           
        for(j=0;j<7;j++)
        {
            vib_nop();          
            recv_data = 0;
            for(i=0;i<8;i++)
            {   
                ADS_SCLK_L();
                vib_nop();
                    
                ADS_SCLK_H();
                vib_nop();	
                
                recv_data<<=1;
                if(ADS_MISO() != 0)
                {
                    recv_data |= 0x01;
                }	
            }
            vib_raw_data_buf[1 + j] = recv_data;
        }
                        
        temp_x = (vib_raw_data_buf[2 + 0]<<0)|(vib_raw_data_buf[2 + 1]<<8);
        temp_y = (vib_raw_data_buf[4 + 0]<<0)|(vib_raw_data_buf[4 + 1]<<8);
        temp_z = ((vib_raw_data_buf[6 + 0]<<0)|(vib_raw_data_buf[6 + 1]<<8));
        
        average_filter_add(&iProfile.vibrate.avg_vib_x,temp_x);
        average_filter_add(&iProfile.vibrate.avg_vib_y,temp_y);
        average_filter_add(&iProfile.vibrate.avg_vib_z,temp_z);

        ACC_CS_H();
        vib_nop();
        ADS_SCLK_L();
        vib_nop();
        
        polling_cnt++;
    }
        
        
    vib_writeByte(0x0A,0x00);//FIFO_CTRL4 :
    vib_nop();
    vib_nop();
    vib_writeByte(0x0A,0x06);//FIFO_CTRL4 :
    vib_nop();
    vib_nop();
    
    //if(polling_cnt >= 2)
    {
        //polling_cnt = 0;      
        //z
        findPeaks(iProfile.vibrate.avg_vib_z.p_avg_buf, VIB_PAKE_LENGTH,4, vib_max_list, &vib_max_list_cnt, vib_min_list, &vib_min_list_cnt,vib_sign,vib_flag_max,vib_idelete,vib_temp_max);  
                
        if(vib_max_list_cnt < vib_min_list_cnt)
        {
            peak_cnt = vib_max_list_cnt;
        }
        else
        {
            peak_cnt = vib_min_list_cnt;
        }
        
        for(i=0;i<peak_cnt;i++)
        {
            temp_32 = iProfile.vibrate.avg_vib_z.p_avg_buf[vib_max_list[i]] - iProfile.vibrate.avg_vib_z.p_avg_buf[vib_min_list[i]];
            average_filter_add(&iProfile.vibrate.avg_vib_result_z,temp_32);  
        }
        
        //x
        findPeaks(iProfile.vibrate.avg_vib_x.p_avg_buf, VIB_PAKE_LENGTH,4, vib_max_list, &vib_max_list_cnt, vib_min_list, &vib_min_list_cnt,vib_sign,vib_flag_max,vib_idelete,vib_temp_max);  
                
        if(vib_max_list_cnt < vib_min_list_cnt)
        {
            peak_cnt = vib_max_list_cnt;
        }
        else
        {
            peak_cnt = vib_min_list_cnt;
        }
        
        for(i=0;i<peak_cnt;i++)
        {
            temp_32 = iProfile.vibrate.avg_vib_x.p_avg_buf[vib_max_list[i]] - iProfile.vibrate.avg_vib_x.p_avg_buf[vib_min_list[i]];
            average_filter_add(&iProfile.vibrate.avg_vib_result_x,temp_32);  
        }
        
        //y
        findPeaks(iProfile.vibrate.avg_vib_y.p_avg_buf, VIB_PAKE_LENGTH,4, vib_max_list, &vib_max_list_cnt, vib_min_list, &vib_min_list_cnt,vib_sign,vib_flag_max,vib_idelete,vib_temp_max);  
                
        if(vib_max_list_cnt < vib_min_list_cnt)
        {
            peak_cnt = vib_max_list_cnt;
        }
        else
        {
            peak_cnt = vib_min_list_cnt;
        }
        
        for(i=0;i<peak_cnt;i++)
        {
            temp_32 = iProfile.vibrate.avg_vib_y.p_avg_buf[vib_max_list[i]] - iProfile.vibrate.avg_vib_y.p_avg_buf[vib_min_list[i]];
            average_filter_add(&iProfile.vibrate.avg_vib_result_y,temp_32);  
        }

        ax2 = average_filter_calc(&iProfile.vibrate.avg_vib_result_x,NULL,NULL);
        ay2 = average_filter_calc(&iProfile.vibrate.avg_vib_result_y,NULL,NULL);
        az2 = average_filter_calc(&iProfile.vibrate.avg_vib_result_z,NULL,NULL);   


        iProfile.vibrate.x_raw = ax2;
        iProfile.vibrate.y_raw = ay2;
        iProfile.vibrate.z_raw = az2;


        iProfile.vibrate.vib_x = dpoint_cal(&iProfile.vibrate.calib[2],ax2);
        iProfile.vibrate.vib_y = dpoint_cal(&iProfile.vibrate.calib[3],ay2);
        iProfile.vibrate.vib_z = dpoint_cal(&iProfile.vibrate.calib[4],az2);

        ax = average_filter_calc(&iProfile.vibrate.avg_vib_x,NULL,NULL);
        ay = average_filter_calc(&iProfile.vibrate.avg_vib_y,NULL,NULL);
        az = average_filter_calc(&iProfile.vibrate.avg_vib_z,NULL,NULL);   
        
        az = az*-1;
        if(az<-8192)
        {
            az = az*-1;
        }
        
        ax2 = atan2s32(ay,az);
        ay2 = atan2s32(-1*ax,az);
        
        ax = ax*10000/16384; 
        ay = ay*10000/16384;
        az = az*10000/16384;
        
        ax2 = low_pass_filter_s16(ax2,&angle_pitch_low_pass_prev,angle_low_filter_alpha);
        ay2 = low_pass_filter_s16(ay2,&angle_roll_low_pass_prev,angle_low_filter_alpha);
        
        iProfile.vibrate.pitch_raw = ax2;
        iProfile.vibrate.roll_raw = ay2;
        
        ax2 = dpoint_cal(&iProfile.vibrate.calib[0],ax2);
        ay2 = dpoint_cal(&iProfile.vibrate.calib[1],ay2);
        
//        ax2 = kalmanFilter_calc(&vib_kalman[0],ax2);
//        ay2 = kalmanFilter_calc(&vib_kalman[1],ay2);
        
        iProfile.vibrate.pitch = ax2;
        iProfile.vibrate.roll = ay2;
    }
    
//    findPeaks(iProfile.vibrate.avg_vib_z.p_avg_buf, VIB_PAKE_LENGTH, 5, vib_max_list, &vib_max_list_cnt, vib_min_list, &vib_min_list_cnt,vib_sign,vib_flag_max,vib_idelete,vib_temp_max);
//    for(i=0;i<4;i++)
//    {
//        vib_max[0] = -65536;
//        vib_min[0] =  65536;
//        
//        vib_max[1] = -65536;
//        vib_min[1] =  65536;
//        
//        vib_max[2] = -65536;
//        vib_min[2] =  65536;
//        
//        for(j=0;j<30;j++)
//        {            
//            if(iProfile.vibrate.avg_vib_x.p_avg_buf[i*10 + j] > vib_max[0])
//            {
//                vib_max[0] = iProfile.vibrate.avg_vib_x.p_avg_buf[i*10 + j];
//            }
//            if(iProfile.vibrate.avg_vib_x.p_avg_buf[i*10 + j] < vib_min[0])
//            {
//                vib_min[0] = iProfile.vibrate.avg_vib_x.p_avg_buf[i*10 + j];
//            }
//            
//            if(iProfile.vibrate.avg_vib_y.p_avg_buf[i*10 + j] > vib_max[1])
//            {
//                vib_max[1] = iProfile.vibrate.avg_vib_y.p_avg_buf[i*10 + j];
//            }
//            if(iProfile.vibrate.avg_vib_y.p_avg_buf[i*10 + j] < vib_min[1])
//            {
//                vib_min[1] = iProfile.vibrate.avg_vib_y.p_avg_buf[i*10 + j];
//            }
//            
//            if(iProfile.vibrate.avg_vib_z.p_avg_buf[i*10 + j] > vib_max[2])
//            {
//                vib_max[2] = iProfile.vibrate.avg_vib_z.p_avg_buf[i*10 + j];
//            }
//            if(iProfile.vibrate.avg_vib_z.p_avg_buf[i*10 + j] < vib_min[2])
//            {
//                vib_min[2] = iProfile.vibrate.avg_vib_z.p_avg_buf[i*10 + j];
//            }                             
//        }
//        average_filter_add(&iProfile.vibrate.avg_vib_result_x,vib_max[0]-vib_min[0]);
//        average_filter_add(&iProfile.vibrate.avg_vib_result_y,vib_max[1]-vib_min[1]);
//        average_filter_add(&iProfile.vibrate.avg_vib_result_z,vib_max[2]-vib_min[2]);    
//    }
    
    
    
    
//    ax = ax*625/1024; 
//    ay = ay*625/1024;
//    az = az*625/1024;
//    angle_low_filter_alpha = 18;
//    vib_high_filter_alpha = 80;
//    ax2 = low_pass_filter_s16(ax,&angle_ax_low_pass_prev,angle_low_filter_alpha);
//    ay2 = low_pass_filter_s16(ay,&angle_ax_low_pass_prev,angle_low_filter_alpha);
//    az2 = low_pass_filter_s16(az,&angle_ax_low_pass_prev,angle_low_filter_alpha);

    
    	 
     
//    if(ax2 > 1800)
//    {
//        ax2 = ax2 - 3600;
//    }
//    
//    if(ay2 > 1800)
//    {
//        ay2 = ay2 - 3600;
//    }
    

    
    
               
//    iProfile.vibrate.pitch = ax2;
//    iProfile.vibrate.roll = ay2;
        
 
    
//    iProfile.vibrate.pitch = ax2;
//    iProfile.vibrate.roll = ay2;
    
    
//    if(vib_pp_point>=VIB_PP_BUF_CNT)
//    {
//        vib_pp_point = 0;
//    }
//    vib_pp_buf[0][vib_pp_point] = vib_max[0]-vib_min[0];
//    vib_pp_buf[1][vib_pp_point] = vib_max[1]-vib_min[1];
//    vib_pp_buf[2][vib_pp_point] = vib_max[2]-vib_min[2];
//    vib_pp_point++;
    
//    ax = 0;
//    ay = 0;
//    az = 0;
//    for(i=0;i<VIB_PP_BUF_CNT;i++)
//    {
//        ax += vib_pp_buf[0][i];
//        ay += vib_pp_buf[1][i];
//        az += vib_pp_buf[2][i];
//    }
//    
//    ax = ax/VIB_PP_BUF_CNT;
//    ay = ay/VIB_PP_BUF_CNT;
//    az = az/VIB_PP_BUF_CNT;
        
//    ax = vib_max[0]-vib_min[0];
//    ay = vib_max[1]-vib_min[1];
//    az = vib_max[2]-vib_min[2];
    
//    if(ax < 0)
//    {
//        ax = 0;
//    }
//    
//    if(ay < 0)
//    {
//        ay = 0;
//    }
//    
//    if(az < 0)
//    {
//        az = 0;
//    }
    
//    ax2 = high_pass_filter_s16(ax,&vib_x_high_pass_prev,vib_high_filter_alpha);
//    ay2 = high_pass_filter_s16(ay,&vib_y_high_pass_prev,vib_high_filter_alpha);
//    az2 = high_pass_filter_s16(az,&vib_z_high_pass_prev,vib_high_filter_alpha);
    
//    ax2 = ax;
//    ay2 = ay;
//    az2 = az;
    
    
    
      
    iProfile.sys_status.v_data_buf[V_ID_ANGLE] = ((ax2&0xFFFF)<<16)|((ay2&0xFFFF)<<0);
    
    
    return 0;
}
    
#else

#endif
